//
// Created by lqk on 2022/1/30.
//

#ifndef QUADCEREBELLUM_CONVEXMPCONTROLLER_H
#define QUADCEREBELLUM_CONVEXMPCONTROLLER_H

#include "OffsetDurationGait.h"
#include "StateEstimator.h"
#include "LegStructs.h"
#include "QuadrupedKinematics.h"
#include "DefaultSwingLegPlanner.h"
#include "EnumStructs.h"
#include "ConvexMPCInterface.h"
#include "chrono"
#include "thread"

class ConvexMPController {
public:
    ConvexMPController(float dt, int loopsPerMPC);
    void Update(EstimatedState *const eState, LegCommand *const legsCommand,LegState *const legsState,QuadrupedKinematics*const quadKine);

private:
    void UpdateMPCIfNeeded(int*,EstimatedState*const,bool);
    void SolveDenseMPC(int*,EstimatedState*const);
    int _loopsPerMPC,_nSegment;
    OffsetDurationGait _trotting,_standing;
    float _dt,_dtMPC;
    int _loopsCounter=0;
    bool _firstRun=true;
    bool _firstSwing[4];
    float _swingTimeRemain[4];
    DefaultSwingLegPlanner _swingPlanner[4];

    float _xVelDes=0.,_yVelDes=0.,_yawVelDes=0.;
    float _bodyHeight=0.25;
    float _rollDes=0.,_pitchDes=0.,_yawDes=0.;
    Eigen::Vector3f _posDes; // Desired x, y, yaw
    Eigen::Vector3f _posFootWorld[4];
    Eigen::Vector3f _footForce[4];

    Eigen::Matrix3f _kp,_kd,_kpStance,_kdStance;
    GaitName _currentGait=GaitName::standing;
    float _trajAll[12*20];
};


#endif //QUADCEREBELLUM_CONVEXMPCONTROLLER_H
